#include <costmap.h>
unsigned char Costmap::get_cost_value(unsigned int x,unsigned int y)
{
    if(x > width_-1 || y > height_-1) return 255;
    return map_[y*width_ + x];
}
bool Costmap::world2map(double wx, double wy, unsigned int &mx, unsigned int &my)
{
    mx = wx/resolution_ + 0.5 + width_/2;
    my = wx/resolution_ + 0.5 + height_/2;
    if(mx > width_-1 || my > height_-1)   
        return false;
    else 
        return true;
}
void Costmap::map2world(unsigned int mx, unsigned int my, double &wx, double &wy)
{
    wx = (mx-width_/2)*resolution_;
    wy = (my-height_/2)*resolution_;
}

void Costmap::init_map(unsigned int h,unsigned int w,double r)
{
    height_ = h;
    width_ = w;
    resolution_ = r;
}